% creating the environment and parameters for occupancy grid mapping
clear all
global p_occ_default ;
global p_empty_default;

global p_occ_sensor;
global p_empty_sensor ; 
global log_odds_occ; 
global log_odds_occ_sensor;
global log_odds_empty_sensor;

 p_occ_default = 0.2; %this can be changed
 p_empty_default = 0.8; %this can be changed

 p_occ_sensor = 0.95; %this can be changed
 p_empty_sensor = 0.85; %this can be changed



max_range = 100; %this can be changed
map_size = [201,201]; %this can be changed

obstacle_list = [100,110;99,110;101,110]; %this can be changed



log_odds_occ = log(p_occ_default/p_empty_default);
log_odds_occ_sensor = log(p_occ_sensor/(1-p_occ_sensor));
log_odds_empty_sensor = log((1-p_empty_sensor)/p_empty_sensor);
robo_map = log_odds_occ*ones(map_size);  
world_map = zeros(map_size);

lin_ind =  sub2ind(size(world_map),obstacle_list(:,1),obstacle_list(:,2));
world_map(lin_ind) = 1;
clear lin_ind


%% running the occupancy grid mapping
for i=0:100
[xy,r,occ] = slow_trace(world_map,deg2rad(200),max_range,[100,100]);
robo_map = update_robo_world(deg2rad(200),r,[100,100],xy,occ,robo_map);
    for j= 1:360
        info_gain(i+1,j) = estimate_info_gain(robo_map,deg2rad(j),max_range,[100,100]);
    end
end
%% displaying results
imshow( world_map)
hold on
plot([100,xy(2)],[100,xy(1)],'c')
hold off
figure
imshow(robo_map - min(min(robo_map)))

clear i
clear j